import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Quaternion
import RPi.GPIO as GPIO

# BOARD mode
PIN_RISE = 5
PIN_RISE_ACT = 6
PIN_EXTEND = 13
PIN_EXTEND_ACT = 16
PIN_ROTATE_LEFT = 19
PIN_ROTATE_RIGHT = 20
PIN_LIFT_UP = 21
PIN_LIFT_DOWN = 26
# 29 31 32 33 35 36 37 38

class RelayControl(Node):

    def __init__(self):
        super().__init__('relay_control')
        self.subscription = self.create_subscription(
            Quaternion,
            '/cranebot/move',
            self.listener_callback,
            10
        )
        self.subscription
        GPIO.setmode(GPIO.BCM)
        GPIO.setup(PIN_RISE, GPIO.OUT)
        GPIO.setup(PIN_RISE_ACT, GPIO.OUT)
        GPIO.setup(PIN_EXTEND, GPIO.OUT)
        GPIO.setup(PIN_EXTEND_ACT, GPIO.OUT)
        GPIO.setup(PIN_ROTATE_LEFT, GPIO.OUT)
        GPIO.setup(PIN_ROTATE_RIGHT, GPIO.OUT)
        GPIO.setup(PIN_LIFT_UP, GPIO.OUT)
        GPIO.setup(PIN_LIFT_DOWN, GPIO.OUT)
        GPIO.output(PIN_RISE, 1)
        GPIO.output(PIN_RISE_ACT, 1)
        GPIO.output(PIN_EXTEND, 1)
        GPIO.output(PIN_EXTEND_ACT, 1)
        GPIO.output(PIN_ROTATE_LEFT, 1)
        GPIO.output(PIN_ROTATE_RIGHT, 1)
        GPIO.output(PIN_LIFT_UP, 1)
        GPIO.output(PIN_LIFT_DOWN, 1)

    def listener_callback(self, msg):
        if ((msg.w == 0.0) & (msg.x == 0.0) & (msg.y == 0.0) & (msg.z == 0.0)):
            GPIO.output(PIN_RISE, 1)
            GPIO.output(PIN_RISE_ACT, 1)
            GPIO.output(PIN_EXTEND, 1)
            GPIO.output(PIN_EXTEND_ACT, 1)
            GPIO.output(PIN_ROTATE_LEFT, 1)
            GPIO.output(PIN_ROTATE_RIGHT, 1)
            GPIO.output(PIN_LIFT_UP, 1)
            GPIO.output(PIN_LIFT_DOWN, 1)
            self.get_logger().info("PAUSE")
        elif ((msg.w == 1.0) & (msg.x == 0.0) & (msg.y == 0.0) & (msg.z == 0.0)):
            GPIO.output(PIN_RISE, 0)
            GPIO.output(PIN_RISE_ACT, 0)
            GPIO.output(PIN_EXTEND, 1)
            GPIO.output(PIN_EXTEND_ACT, 1)
            GPIO.output(PIN_ROTATE_LEFT, 1)
            GPIO.output(PIN_ROTATE_RIGHT, 1)
            GPIO.output(PIN_LIFT_UP, 1)
            GPIO.output(PIN_LIFT_DOWN, 1)
            self.get_logger().info("Rise UP")
        elif ((msg.w == -1.0) & (msg.x == 0.0) & (msg.y == 0.0) & (msg.z == 0.0)):
            GPIO.output(PIN_RISE, 1)
            GPIO.output(PIN_RISE_ACT, 0)
            GPIO.output(PIN_EXTEND, 1)
            GPIO.output(PIN_EXTEND_ACT, 1)
            GPIO.output(PIN_ROTATE_LEFT, 1)
            GPIO.output(PIN_ROTATE_RIGHT, 1)
            GPIO.output(PIN_LIFT_UP, 1)
            GPIO.output(PIN_LIFT_DOWN, 1)        
            self.get_logger().info("Fall DOWN")
        elif ((msg.w == 0.0) & (msg.x == 1.0) & (msg.y == 0.0) & (msg.z == 0.0)):
            GPIO.output(PIN_RISE, 1)
            GPIO.output(PIN_RISE_ACT, 1)
            GPIO.output(PIN_EXTEND, 0)
            GPIO.output(PIN_EXTEND_ACT, 0)
            GPIO.output(PIN_ROTATE_LEFT, 1)
            GPIO.output(PIN_ROTATE_RIGHT, 1)
            GPIO.output(PIN_LIFT_UP, 1)
            GPIO.output(PIN_LIFT_DOWN, 1)        
            self.get_logger().info("Extend FORAWARD")
        elif ((msg.w == 0.0) & (msg.x == -1.0) & (msg.y == 0.0) & (msg.z == 0.0)):
            GPIO.output(PIN_RISE, 1)
            GPIO.output(PIN_RISE_ACT, 1)
            GPIO.output(PIN_EXTEND, 1)
            GPIO.output(PIN_EXTEND_ACT, 0)
            GPIO.output(PIN_ROTATE_LEFT, 1)
            GPIO.output(PIN_ROTATE_RIGHT, 1)
            GPIO.output(PIN_LIFT_UP, 1)
            GPIO.output(PIN_LIFT_DOWN, 1)        
            self.get_logger().info("Extend BACKWARD")
        elif ((msg.w == 0.0) & (msg.x == 0.0) & (msg.y == 1.0) & (msg.z == 0.0)):
            GPIO.output(PIN_RISE, 1)
            GPIO.output(PIN_RISE_ACT, 1)
            GPIO.output(PIN_EXTEND, 1)
            GPIO.output(PIN_EXTEND_ACT, 1)
            GPIO.output(PIN_ROTATE_LEFT, 0)
            GPIO.output(PIN_ROTATE_RIGHT, 1)
            GPIO.output(PIN_LIFT_UP, 1)
            GPIO.output(PIN_LIFT_DOWN, 1)        
            self.get_logger().info("Rotate LEFT")
        elif ((msg.w == 0.0) & (msg.x == 0.0) & (msg.y == -1.0) & (msg.z == 0.0)):
            GPIO.output(PIN_RISE, 1)
            GPIO.output(PIN_RISE_ACT, 1)
            GPIO.output(PIN_EXTEND, 1)
            GPIO.output(PIN_EXTEND_ACT, 1)
            GPIO.output(PIN_ROTATE_LEFT, 1)
            GPIO.output(PIN_ROTATE_RIGHT, 0)
            GPIO.output(PIN_LIFT_UP, 1)
            GPIO.output(PIN_LIFT_DOWN, 1)        
            self.get_logger().info("Rotate RIGHT")
        elif ((msg.w == 0.0) & (msg.x == 0.0) & (msg.y == 0.0) & (msg.z == 1.0)):
            GPIO.output(PIN_RISE, 1)
            GPIO.output(PIN_RISE_ACT, 1)
            GPIO.output(PIN_EXTEND, 1)
            GPIO.output(PIN_EXTEND_ACT, 1)
            GPIO.output(PIN_ROTATE_LEFT, 1)
            GPIO.output(PIN_ROTATE_RIGHT, 1)
            GPIO.output(PIN_LIFT_UP, 1)
            GPIO.output(PIN_LIFT_DOWN, 0)        
            self.get_logger().info("Lift UP")
        elif ((msg.w == 0.0) & (msg.x == 0.0) & (msg.y == 0.0) & (msg.z == -1.0)):
            GPIO.output(PIN_RISE, 1)
            GPIO.output(PIN_RISE_ACT, 1)
            GPIO.output(PIN_EXTEND, 1)
            GPIO.output(PIN_EXTEND_ACT, 1)
            GPIO.output(PIN_ROTATE_LEFT, 1)
            GPIO.output(PIN_ROTATE_RIGHT, 1)
            GPIO.output(PIN_LIFT_UP, 0)
            GPIO.output(PIN_LIFT_DOWN, 1)        
            self.get_logger().info("Lift DOWN")


def main(args=None):
    rclpy.init(args=args)
    relay_control = RelayControl()
    try:
        rclpy.spin(relay_control)
    except KeyboardInterrupt:
        GPIO.cleanup()
    relay_control.destroy_node()
    GPIO.cleanup()
    print('Shut down------------------------')
    rclpy.shutdown()

if __name__ == '__main__':
    main()